#include "link.hpp"
#include "packet.hpp"
#include "packet_data_writer.hpp"
#include "packet_data_reader.hpp"
#include "node.hpp"

#include <gvl/support/debug.hpp>
#include <gvl/support/log.hpp>
#include <gvl/system/system.hpp>
#include <gvl/support/bits.hpp>
#include <algorithm>


namespace Psync5
{

uint32_t const magicWord = 0x5;


void ReliableLayer::send(Packet const& packet)
{
	TLOG(this << ": sending packet, size " << packet.size());
	prevPacketTime_ = gvl::get_ticks();

	node->send(packet, addr);
	// TODO: If send failed, we should try to resend very shortly
}

void ReliableLayer::sendInit_()
{
	uint8_t data[2048];
	Packet initPacket(data, data + 2048);

	initPacket.writeHeader(PacketHeader(PacketHeader::Init, nextPacketId++));
	initPacket.write32(magicWord);

	send(initPacket);
}

void ReliableLayer::sendCookie_(bool sendData)
{
	uint8_t buf[256];
	
	Packet cookie(buf, buf + 256);
	cookie.writeHeader(PacketHeader(PacketHeader::Cookie, 0));

	cookie.write64(recvCookie.x);
	cookie.write64(recvCookie.y);
	cookie.write32(recvCookie.timer);

	// TODO: Add data if sendData is true

	send(cookie);
}

void ReliableLayer::sendShutdown_()
{
	PacketStorage shutdownPacket;
	shutdownPacket.writeHeader(PacketHeader(PacketHeader::Shutdown, nextPacketId++));
	send(shutdownPacket);
}

void ReliableLayer::sendShutdownAck_()
{
	PacketStorage shutdownAckPacket;
	shutdownAckPacket.writeHeader(PacketHeader(PacketHeader::ShutdownAck, nextPacketId++));
	send(shutdownAckPacket);
}

void ReliableLayer::sendShutdownComplete_()
{
	PacketStorage shutdownCompletePacket;
	shutdownCompletePacket.writeHeader(PacketHeader(PacketHeader::ShutdownComplete, nextPacketId++));
	send(shutdownCompletePacket);
}

void ReliableLayer::resetRecv_()
{
	channels[0].recvQueue.reset(0);
}

void ReliableLayer::resetSend_()
{
	channels[0].sendQueue.clear();
	channels[0].resendQueue.clear();
	channels[0].nextSectionId = 0;
}

void ReliableLayer::disconnect()
{
	if(state != Disconnected
	&& state != ShutdownWaitAck)
	{
		sendShutdown_();
		if(state != ShutdownWaitAck)
		{
			resetSend_();
			resetRecv_();
		}
		state = ShutdownWaitAck;
	}
}

void ReliableLayer::handleInitAck(Packet packet)
{
	if(state != Initiated)
		return;

	recvCookie.x = packet.read64();
	recvCookie.y = packet.read64();
	recvCookie.timer = packet.read32();
	
	sendCookie_(true);

	state = Cookied;
}

void ReliableLayer::handleShutdown(Packet packet)
{
	if(state != Disconnected
	&& state != ShutdownWaitAck)
	{
		if(state != ShutdownWaitComplete)
		{
			resetSend_();
			resetRecv_();
		}
		sendShutdownAck_();
		state = ShutdownWaitComplete;
	}
}

void ReliableLayer::handleShutdownAck(Packet packet)
{
	if(state == ShutdownWaitAck)
	{
		sendShutdownComplete_();
		state = Disconnected;
	}
}

void ReliableLayer::handleShutdownComplete(Packet packet)
{
	if(state == ShutdownWaitComplete)
	{
		state = Disconnected;
	}
}

bool ReliableLayer::handleCookie()
{
	bool wasNotConnected = (state == Disconnected);

	state = Connected;

	TLOG(this << ": received valid Cookie");
	uint8_t buf[256];
	Packet cookieAck(buf, buf + 256);
	cookieAck.writeHeader(PacketHeader(PacketHeader::CookieAck, 0));

	cookieAck.trySend(sock, addr);

	if(wasNotConnected)
		resetRecv_();

	// TODO: Recv data sent with Cookie?
	
	return wasNotConnected;
}

bool ReliableLayer::handleCookieAck(Packet packet)
{
	if(state != Cookied)
		return false;

	state = Connected;
	prevPacketTime_ = gvl::get_ticks();

	resetRecv_();
	// TODO: Recv data sent with CookieAck?
	
	return true;
}

void ReliableLayer::initiate()
{
	sendInit_();

	state = Initiated;
}

bool ReliableLayer::tryAddSections_(PacketDataWriter& writer, bool& anythingToSend)
{
	Channel& channel = channels[0];

	while(!channel.sendQueue.empty())
	{
		auto first = channel.sendQueue.first();
		if(!writer.addSection(*first))
			return false;

		anythingToSend = true;
		// Move to resend queue
		channel.sendQueue.unlink_front();
		channel.resendQueue.relink_back(first);
	}

	return true;
}

bool ReliableLayer::assemblePacket_(Packet& destPacket)
{
	PacketDataWriter writer(destPacket, 1);

	Channel& channel = channels[0];

	bool anythingToSend = false;

	auto now = gvl::get_ticks();

	int const resendTimeOut = 1000;

	/*
		sssnnnn0 ssssssss 000sssss   -> channel c, section [s, s + n]

		-------1 ... -> last
	*/

	std::sort(pendingAcks.begin(), pendingAcks.end());

	while(!pendingAcks.empty() && destPacket.spaceLeft() >= 3)
	{
		uint32_t first = pendingAcks.front();
		uint32_t count = 0;

		pendingAcks.pop_front();

		while(!pendingAcks.empty())
		{
			uint32_t id = pendingAcks.front();

			if(gvl::cyclic_between(first, first + count + 1, id, 0xffff))
			{
				// Already accounted for, do nothing
			}
			else if(id == ((first + count + 1) & 0xffff))
			{
				if(count == 127) // Can't compress any more
					break;

				pendingAcks.pop_front();

				++count;
			}
			else
			{
				// Can't accomodate this
				break;
			}
		}

		TLOG("Sent ack for [" << first << ", " << ((first + count) & 0xffff) << "]");

		destPacket.write24((first << 8) | (count << 1));
		anythingToSend = true;
	}

	// Ack terminator.
	// TODO: 7 unused bits here
	destPacket.write8(1);

#if 1
	
	for(auto r = channel.resendQueue.all(); !r.empty(); r.pop_front())
	{
		if((now - r.front().lastSendMs) >= 10000) // resendTimeOut)
		{
			if(writer.addSection(r.front()))
			{
				//LOG("Resending section " << r.front().id);
				anythingToSend = true;
			}
		}
	}
#endif

	if(tryAddSections_(writer, anythingToSend))
	{
		// We can potentially fit more, flush
		// and try again.
		flush();
		tryAddSections_(writer, anythingToSend);
	}

	if(anythingToSend)
		writer.finish();

	return anythingToSend;
}

void ReliableLayer::assembleAndSendPacket()
{
	if(state != Connected)
		return; // Can't send normal packets

	PacketStorage packet;
	
	packet.writeHeader(PacketHeader(PacketHeader::Normal, nextPacketId));
	if(!assemblePacket_(packet))
		return; // Nothing to send
	++nextPacketId;
	send(packet);
}

void ReliableLayer::reportAckLatency_(uint32_t ms)
{
	latencyHistory[latencyHistoryPos] = ms;
	latencyHistoryPos = (latencyHistoryPos + 1) % LatencyHistorySize;
	if(latencyHistoryCount < LatencyHistorySize)
		++latencyHistoryCount;
}

float ReliableLayer::calcLatency()
{
	if(latencyHistoryCount == 0)
		return -1.f;

	uint32_t first = (latencyHistoryPos - latencyHistoryCount) % LatencyHistorySize;

	int sum = 0;

	for(uint32_t i = 0; i < latencyHistoryCount; ++i)
		sum += latencyHistory[(first + i) % LatencyHistorySize];

	float mean = sum / float(latencyHistoryCount);

	float variance = 0.0;
	for(uint32_t i = 0; i < latencyHistoryCount; ++i)
	{
		float v = latencyHistory[(first + i) % LatencyHistorySize] - mean;
		variance += v*v;
	}

	variance /= float(latencyHistoryCount);

	// TODO: Exclude outliers

	return mean;
}

void ReliableLayer::recv(Packet packet)
{
	if(!allowsReceiving())
		return; // Ignore

	PacketDataReader reader(packet, 1);

	TLOG(this << ": received normal packet, size " << packet.size());

	uint32_t initialSectionCount = reader.sectionCount();

	/*
		nnnnnnn0 ssssssss ssssssss   -> channel c, section [s, s + n]

		-------1 ... -> last
	*/

	uint32_t now = gvl::get_ticks();

	// TODO: Something seems wrong with the ack sending, as a lot of sections
	// are not removed from resendQueue even locally when all sends are successful.

	while(true)
	{
		uint32_t w = packet.read8();
		if(w & 1)
			break; // No more acks
		uint32_t first = packet.read16();

		uint32_t count = (w >> 1);

		TLOG("Received ack for [" << first << ", " << ((first + count) & 0xffff) << "]");

		uint32_t sectionId = first & 0xffff;

		Channel& ch = channel();

		for(auto r = ch.resendQueue.all(); !r.empty();)
		{
			uint32_t id = r.front().id;
			if(gvl::cyclic_between(sectionId, sectionId + count + 1, id))
			{
				TLOG("Marked " << id << " as sent");
				reportAckLatency_(now - r.front().lastSendMs);
				ch.resendQueue.erase_front(r);
			}
			else
			{
				r.pop_front();
			}
		}
	}

	for(uint32_t i = 0; i < initialSectionCount; ++i)
	{
		std::unique_ptr<Section> section(reader.readSection());

		Channel& ch = channels[0];

		uint32_t ack = section->id;
		pendingAcks.push_back(ack);

		ch.recvQueue.addSection(std::move(section));

	}
}

bool ReliableLayer::packetTimedOut_(uint32_t timeOut)
{
	return (gvl::get_ticks() - prevPacketTime_) >= timeOut;
}

bool ReliableLayer::isTimeForPacket()
{
	if(!node->automaticPacketSending())
		return false;

	// TODO: Better packet scheduling
	uint32_t now = gvl::get_ticks();

	if(gvl::udiff(prevPacketTime_ + 100, now) < 0)
		return true;

	return false;
}

void ReliableLayer::process()
{
	switch(state)
	{
		case Initiated:
		{
			if(packetTimedOut_(1000))
			{
				// Send Init again
				sendInit_();
			}

			break;
		}

		case Cookied:
		{
			if(packetTimedOut_(1000))
			{
				// Send Cookie again, but don't send
				// any data this time
				sendCookie_(false);
			}

			break;
		}

		case Connected:
		{
			/* Nothing */
			break;
		}

		case ShutdownWaitAck:
		{
			if(packetTimedOut_(1000))
			{
				// Send Shutdown again
				sendShutdown_();
			}
			break;
		}

		case ShutdownWaitComplete:
		{
			if(packetTimedOut_(1000))
			{
				// Send ShutdownAck again
				sendShutdownAck_();
			}
			break;
		}

		case Disconnected:
			/* Nothing */
			break;
	}

	if(isTimeForPacket())
		assembleAndSendPacket();
}

}
